
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN"
  "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta http-equiv="X-UA-Compatible" content="IE=Edge" />
    <meta http-equiv="Content-Type" content="text/html; charset=utf-8" />
    <title>pypcd.numpy_pc2 &#8212; pypcd 0.1.1 documentation</title>
    <link rel="stylesheet" href="../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
    <script type="text/javascript" src="../../_static/documentation_options.js"></script>
    <script type="text/javascript" src="../../_static/jquery.js"></script>
    <script type="text/javascript" src="../../_static/underscore.js"></script>
    <script type="text/javascript" src="../../_static/doctools.js"></script>
    <link rel="index" title="Index" href="../../genindex.html" />
    <link rel="search" title="Search" href="../../search.html" />
   
  <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for pypcd.numpy_pc2</h1><div class="highlight"><pre>
<span></span><span class="c1"># Software License Agreement (BSD License)</span>
<span class="c1">#</span>
<span class="c1"># Copyright (c) 2008, Willow Garage, Inc.</span>
<span class="c1"># All rights reserved.</span>
<span class="c1">#</span>
<span class="c1"># Redistribution and use in source and binary forms, with or without</span>
<span class="c1"># modification, are permitted provided that the following conditions</span>
<span class="c1"># are met:</span>
<span class="c1">#</span>
<span class="c1">#  * Redistributions of source code must retain the above copyright</span>
<span class="c1">#    notice, this list of conditions and the following disclaimer.</span>
<span class="c1">#  * Redistributions in binary form must reproduce the above</span>
<span class="c1">#    copyright notice, this list of conditions and the following</span>
<span class="c1">#    disclaimer in the documentation and/or other materials provided</span>
<span class="c1">#    with the distribution.</span>
<span class="c1">#  * Neither the name of Willow Garage, Inc. nor the names of its</span>
<span class="c1">#    contributors may be used to endorse or promote products derived</span>
<span class="c1">#    from this software without specific prior written permission.</span>
<span class="c1">#</span>
<span class="c1"># THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS</span>
<span class="c1"># &quot;AS IS&quot; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT</span>
<span class="c1"># LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS</span>
<span class="c1"># FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE</span>
<span class="c1"># COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,</span>
<span class="c1"># INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,</span>
<span class="c1"># BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</span>
<span class="c1"># LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER</span>
<span class="c1"># CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT</span>
<span class="c1"># LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN</span>
<span class="c1"># ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE</span>
<span class="c1"># POSSIBILITY OF SUCH DAMAGE.</span>
<span class="c1">#</span>
<span class="c1"># Author: Jon Binney</span>
<span class="c1"># Updates: Daniel Maturana</span>

<span class="sd">&#39;&#39;&#39;</span>
<span class="sd">Functions for working with PointCloud2.</span>
<span class="sd">&#39;&#39;&#39;</span>
<span class="n">__docformat__</span> <span class="o">=</span> <span class="s2">&quot;restructuredtext en&quot;</span>

<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">import</span> <span class="nn">numpy.lib.recfunctions</span> <span class="k">as</span> <span class="nn">recfuncs</span>

<span class="kn">from</span> <span class="nn">sensor_msgs.msg</span> <span class="k">import</span> <span class="n">PointField</span>
<span class="kn">from</span> <span class="nn">sensor_msgs.msg</span> <span class="k">import</span> <span class="n">PointCloud2</span>

<span class="c1"># prefix to the names of dummy fields we add to get byte alignment correct. this needs to not</span>
<span class="c1"># clash with any actual field names</span>
<span class="n">DUMMY_FIELD_PREFIX</span> <span class="o">=</span> <span class="s1">&#39;__&#39;</span>

<span class="c1"># mappings between PointField types and numpy types</span>
<span class="n">type_mappings</span> <span class="o">=</span> <span class="p">[(</span><span class="n">PointField</span><span class="o">.</span><span class="n">INT8</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;int8&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">UINT8</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;uint8&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">INT16</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;int16&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">UINT16</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;uint16&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">INT32</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;int32&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">UINT32</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;uint32&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">FLOAT32</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;float32&#39;</span><span class="p">)),</span>
                 <span class="p">(</span><span class="n">PointField</span><span class="o">.</span><span class="n">FLOAT64</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">(</span><span class="s1">&#39;float64&#39;</span><span class="p">))]</span>

<span class="n">pftype_to_nptype</span> <span class="o">=</span> <span class="nb">dict</span><span class="p">(</span><span class="n">type_mappings</span><span class="p">)</span>
<span class="n">nptype_to_pftype</span> <span class="o">=</span> <span class="nb">dict</span><span class="p">((</span><span class="n">nptype</span><span class="p">,</span> <span class="n">pftype</span><span class="p">)</span> <span class="k">for</span> <span class="n">pftype</span><span class="p">,</span> <span class="n">nptype</span> <span class="ow">in</span> <span class="n">type_mappings</span><span class="p">)</span>

<span class="c1"># sizes (in bytes) of PointField types</span>
<span class="n">pftype_sizes</span> <span class="o">=</span> <span class="p">{</span><span class="n">PointField</span><span class="o">.</span><span class="n">INT8</span><span class="p">:</span> <span class="mi">1</span><span class="p">,</span> <span class="n">PointField</span><span class="o">.</span><span class="n">UINT8</span><span class="p">:</span> <span class="mi">1</span><span class="p">,</span> <span class="n">PointField</span><span class="o">.</span><span class="n">INT16</span><span class="p">:</span> <span class="mi">2</span><span class="p">,</span> <span class="n">PointField</span><span class="o">.</span><span class="n">UINT16</span><span class="p">:</span> <span class="mi">2</span><span class="p">,</span>
                <span class="n">PointField</span><span class="o">.</span><span class="n">INT32</span><span class="p">:</span> <span class="mi">4</span><span class="p">,</span> <span class="n">PointField</span><span class="o">.</span><span class="n">UINT32</span><span class="p">:</span> <span class="mi">4</span><span class="p">,</span> <span class="n">PointField</span><span class="o">.</span><span class="n">FLOAT32</span><span class="p">:</span> <span class="mi">4</span><span class="p">,</span> <span class="n">PointField</span><span class="o">.</span><span class="n">FLOAT64</span><span class="p">:</span> <span class="mi">8</span><span class="p">}</span>

<div class="viewcode-block" id="pointfields_to_dtype"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.pointfields_to_dtype">[docs]</a><span class="k">def</span> <span class="nf">pointfields_to_dtype</span><span class="p">(</span><span class="n">point_fields</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Convert a list of PointFields to a numpy record datatype.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="n">offset</span> <span class="o">=</span> <span class="mi">0</span>
    <span class="n">np_dtype_list</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">f</span> <span class="ow">in</span> <span class="n">point_fields</span><span class="p">:</span>
        <span class="k">while</span> <span class="n">offset</span> <span class="o">&lt;</span> <span class="n">f</span><span class="o">.</span><span class="n">offset</span><span class="p">:</span>
            <span class="c1"># might be extra padding between fields</span>
            <span class="n">np_dtype_list</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;</span><span class="si">%s%d</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="n">DUMMY_FIELD_PREFIX</span><span class="p">,</span> <span class="n">offset</span><span class="p">),</span> <span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">))</span>
            <span class="n">offset</span> <span class="o">+=</span> <span class="mi">1</span>
        <span class="n">np_dtype_list</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="n">f</span><span class="o">.</span><span class="n">name</span><span class="p">,</span> <span class="n">pftype_to_nptype</span><span class="p">[</span><span class="n">f</span><span class="o">.</span><span class="n">datatype</span><span class="p">]))</span>
        <span class="n">offset</span> <span class="o">+=</span> <span class="n">pftype_sizes</span><span class="p">[</span><span class="n">f</span><span class="o">.</span><span class="n">datatype</span><span class="p">]</span>

    <span class="c1"># might be extra padding between points</span>
    <span class="c1">#while offset &lt; cloud_msg.point_step:</span>
        <span class="c1">#np_dtype_list.append((&#39;%s%d&#39; % (DUMMY_FIELD_PREFIX, offset), np.uint8))</span>
        <span class="c1">#offset += 1</span>
</div>
    <span class="k">return</span> <span class="n">np_dtype_list</span>

<div class="viewcode-block" id="pointcloud2_to_dtype"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.pointcloud2_to_dtype">[docs]</a><span class="k">def</span> <span class="nf">pointcloud2_to_dtype</span><span class="p">(</span><span class="n">cloud_msg</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Convert a list of PointFields to a numpy record datatype.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="n">offset</span> <span class="o">=</span> <span class="mi">0</span>
    <span class="n">np_dtype_list</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">f</span> <span class="ow">in</span> <span class="n">cloud_msg</span><span class="o">.</span><span class="n">fields</span><span class="p">:</span>
        <span class="k">while</span> <span class="n">offset</span> <span class="o">&lt;</span> <span class="n">f</span><span class="o">.</span><span class="n">offset</span><span class="p">:</span>
            <span class="c1"># might be extra padding between fields</span>
            <span class="n">np_dtype_list</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;</span><span class="si">%s%d</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="n">DUMMY_FIELD_PREFIX</span><span class="p">,</span> <span class="n">offset</span><span class="p">),</span> <span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">))</span>
            <span class="n">offset</span> <span class="o">+=</span> <span class="mi">1</span>
        <span class="n">np_dtype_list</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="n">f</span><span class="o">.</span><span class="n">name</span><span class="p">,</span> <span class="n">pftype_to_nptype</span><span class="p">[</span><span class="n">f</span><span class="o">.</span><span class="n">datatype</span><span class="p">]))</span>
        <span class="n">offset</span> <span class="o">+=</span> <span class="n">pftype_sizes</span><span class="p">[</span><span class="n">f</span><span class="o">.</span><span class="n">datatype</span><span class="p">]</span>

    <span class="c1"># might be extra padding between points</span>
    <span class="k">while</span> <span class="n">offset</span> <span class="o">&lt;</span> <span class="n">cloud_msg</span><span class="o">.</span><span class="n">point_step</span><span class="p">:</span>
        <span class="n">np_dtype_list</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;</span><span class="si">%s%d</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="n">DUMMY_FIELD_PREFIX</span><span class="p">,</span> <span class="n">offset</span><span class="p">),</span> <span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">))</span>
        <span class="n">offset</span> <span class="o">+=</span> <span class="mi">1</span>
</div>
    <span class="k">return</span> <span class="n">np_dtype_list</span>

<div class="viewcode-block" id="arr_to_fields"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.arr_to_fields">[docs]</a><span class="k">def</span> <span class="nf">arr_to_fields</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Convert a numpy record datatype into a list of PointFields.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="n">fields</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">field_name</span> <span class="ow">in</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">names</span><span class="p">:</span>
        <span class="n">np_field_type</span><span class="p">,</span> <span class="n">field_offset</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">fields</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span>
        <span class="n">pf</span> <span class="o">=</span> <span class="n">PointField</span><span class="p">()</span>
        <span class="n">pf</span><span class="o">.</span><span class="n">name</span> <span class="o">=</span> <span class="n">field_name</span>
        <span class="n">pf</span><span class="o">.</span><span class="n">datatype</span> <span class="o">=</span> <span class="n">nptype_to_pftype</span><span class="p">[</span><span class="n">np_field_type</span><span class="p">]</span>
        <span class="n">pf</span><span class="o">.</span><span class="n">offset</span> <span class="o">=</span> <span class="n">field_offset</span>
        <span class="n">pf</span><span class="o">.</span><span class="n">count</span> <span class="o">=</span> <span class="mi">1</span> <span class="c1"># is this ever more than one?</span>
        <span class="n">fields</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">pf</span><span class="p">)</span></div>
    <span class="k">return</span> <span class="n">fields</span>

<div class="viewcode-block" id="pointcloud2_to_array"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.pointcloud2_to_array">[docs]</a><span class="k">def</span> <span class="nf">pointcloud2_to_array</span><span class="p">(</span><span class="n">cloud_msg</span><span class="p">,</span> <span class="n">split_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">remove_padding</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39; Converts a rospy PointCloud2 message to a numpy recordarray</span>

<span class="sd">    Reshapes the returned array to have shape (height, width), even if the height is 1.</span>

<span class="sd">    The reason for using np.fromstring rather than struct.unpack is speed... especially</span>
<span class="sd">    for large point clouds, this will be &lt;much&gt; faster.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="c1"># construct a numpy record type equivalent to the point type of this cloud</span>
    <span class="n">dtype_list</span> <span class="o">=</span> <span class="n">pointcloud2_to_dtype</span><span class="p">(</span><span class="n">cloud_msg</span><span class="p">)</span>

    <span class="c1"># parse the cloud into an array</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">fromstring</span><span class="p">(</span><span class="n">cloud_msg</span><span class="o">.</span><span class="n">data</span><span class="p">,</span> <span class="n">dtype_list</span><span class="p">)</span>

    <span class="c1"># remove the dummy fields that were added</span>
    <span class="k">if</span> <span class="n">remove_padding</span><span class="p">:</span>
        <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">recfuncs</span><span class="o">.</span><span class="n">repack_fields</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">[</span>
            <span class="p">[</span><span class="n">fname</span> <span class="k">for</span> <span class="n">fname</span><span class="p">,</span> <span class="n">_type</span> <span class="ow">in</span> <span class="n">dtype_list</span> <span class="k">if</span> <span class="ow">not</span> <span class="p">(</span><span class="n">fname</span><span class="p">[:</span><span class="nb">len</span><span class="p">(</span><span class="n">DUMMY_FIELD_PREFIX</span><span class="p">)]</span> <span class="o">==</span> <span class="n">DUMMY_FIELD_PREFIX</span><span class="p">)]])</span>

    <span class="k">if</span> <span class="n">split_rgb</span><span class="p">:</span>
        <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">split_rgb_field</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">)</span>
</div>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="p">(</span><span class="n">cloud_msg</span><span class="o">.</span><span class="n">height</span><span class="p">,</span> <span class="n">cloud_msg</span><span class="o">.</span><span class="n">width</span><span class="p">))</span>

<div class="viewcode-block" id="array_to_xyz_pointcloud2f"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.array_to_xyz_pointcloud2f">[docs]</a><span class="k">def</span> <span class="nf">array_to_xyz_pointcloud2f</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; convert an Nx3 float array to an xyz point cloud.</span>
<span class="sd">    beware of numerical issues when casting from other types to float32.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">ndim</span><span class="o">==</span><span class="mi">2</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr must be 2D array&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">==</span><span class="mi">3</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr shape must be Nx3&#39;</span><span class="p">)</span>
    <span class="n">xyz</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">view</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">([(</span><span class="s1">&#39;x&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;y&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;z&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)]))</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span></div>
    <span class="k">return</span> <span class="n">array_to_pointcloud2</span><span class="p">(</span><span class="n">xyz</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="n">stamp</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="n">frame_id</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="n">merge_rgb</span><span class="p">)</span>

<div class="viewcode-block" id="array_to_xyzi_pointcloud2f"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.array_to_xyzi_pointcloud2f">[docs]</a><span class="k">def</span> <span class="nf">array_to_xyzi_pointcloud2f</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; convert an Nx4 float array to an xyzi point cloud.</span>
<span class="sd">    beware of numerical issues when casting from other types to float32.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">ndim</span><span class="o">==</span><span class="mi">2</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr must be 2D array&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">==</span><span class="mi">4</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr shape must be Nx4&#39;</span><span class="p">)</span>
    <span class="n">xyzi</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">view</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">([</span>
        <span class="p">(</span><span class="s1">&#39;x&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;y&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;z&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;intensity&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
        <span class="p">]))</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span></div>
    <span class="k">return</span> <span class="n">array_to_pointcloud2</span><span class="p">(</span><span class="n">xyzi</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="n">stamp</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="n">frame_id</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="n">merge_rgb</span><span class="p">)</span>

<div class="viewcode-block" id="arrays_to_xyzi_pointcloud2f"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.arrays_to_xyzi_pointcloud2f">[docs]</a><span class="k">def</span> <span class="nf">arrays_to_xyzi_pointcloud2f</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">intensity_array</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; convert an Nx3 float array and N array to an xyzi point cloud.</span>
<span class="sd">    beware of numerical issues when casting from other types to float32.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">ndim</span><span class="o">==</span><span class="mi">2</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr must be 2D array&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">==</span><span class="mi">3</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr shape must be Nx3&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">intensity_array</span><span class="o">.</span><span class="n">size</span> <span class="o">==</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;wrong intensity shape&#39;</span><span class="p">)</span>
    <span class="n">xyzi</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span> <span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">),</span> <span class="mi">4</span><span class="p">)</span> <span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span> <span class="p">)</span>
    <span class="n">xyzi</span><span class="p">[:,</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">cloud_arr</span>
    <span class="n">xyzi</span><span class="p">[:,</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">intensity_array</span>
    <span class="n">xyzi</span> <span class="o">=</span> <span class="n">xyzi</span><span class="o">.</span><span class="n">view</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">([</span>
        <span class="p">(</span><span class="s1">&#39;x&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;y&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;z&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;intensity&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
        <span class="p">]))</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span></div>
    <span class="k">return</span> <span class="n">array_to_pointcloud2</span><span class="p">(</span><span class="n">xyzi</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="n">stamp</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="n">frame_id</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="n">merge_rgb</span><span class="p">)</span>

<div class="viewcode-block" id="array_to_xyzl_pointcloud2f"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.array_to_xyzl_pointcloud2f">[docs]</a><span class="k">def</span> <span class="nf">array_to_xyzl_pointcloud2f</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; convert an Nx4 float array to an xyzi point cloud.</span>
<span class="sd">    beware of numerical issues when casting from other types to float32.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">ndim</span><span class="o">==</span><span class="mi">2</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr must be 2D array&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">==</span><span class="mi">4</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr shape must be Nx3&#39;</span><span class="p">)</span>
    <span class="n">xyzi</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">view</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">([</span>
        <span class="p">(</span><span class="s1">&#39;x&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;y&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;z&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;intensity&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
        <span class="p">]))</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span></div>
    <span class="k">return</span> <span class="n">array_to_pointcloud2</span><span class="p">(</span><span class="n">xyzi</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="n">stamp</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="n">frame_id</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="n">merge_rgb</span><span class="p">)</span>


<div class="viewcode-block" id="array_to_xyz_pointcloud2"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.array_to_xyz_pointcloud2">[docs]</a><span class="k">def</span> <span class="nf">array_to_xyz_pointcloud2</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; convert an Nx3 float array to an xyz point cloud.</span>
<span class="sd">    preserves (scalar) dtype of input.</span>
<span class="sd">    TODO: untested</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">ndim</span><span class="o">==</span><span class="mi">2</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr must be 2D array&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">==</span><span class="mi">3</span><span class="p">:</span> <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;cloud_arr shape must be Nx3&#39;</span><span class="p">)</span>
    <span class="n">xyz</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">view</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dtype</span><span class="p">([(</span><span class="s1">&#39;x&#39;</span><span class="p">,</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;y&#39;</span><span class="p">,</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="p">),</span> <span class="p">(</span><span class="s1">&#39;z&#39;</span><span class="p">,</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="p">)]))</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span></div>
    <span class="k">return</span> <span class="n">array_to_pointcloud2</span><span class="p">(</span><span class="n">xyz</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="n">stamp</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="n">frame_id</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="n">merge_rgb</span><span class="p">)</span>

<div class="viewcode-block" id="array_to_pointcloud2"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.array_to_pointcloud2">[docs]</a><span class="k">def</span> <span class="nf">array_to_pointcloud2</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">,</span> <span class="n">stamp</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">frame_id</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">merge_rgb</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Converts a numpy record array to a sensor_msgs.msg.PointCloud2.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="k">if</span> <span class="n">merge_rgb</span><span class="p">:</span>
        <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">merge_rgb_fields</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">)</span>

    <span class="c1"># make it 2d (even if height will be 1)</span>
    <span class="n">cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">atleast_2d</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">)</span>

    <span class="n">cloud_msg</span> <span class="o">=</span> <span class="n">PointCloud2</span><span class="p">()</span>

    <span class="k">if</span> <span class="n">stamp</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">cloud_msg</span><span class="o">.</span><span class="n">header</span><span class="o">.</span><span class="n">stamp</span> <span class="o">=</span> <span class="n">stamp</span>
    <span class="k">if</span> <span class="n">frame_id</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">cloud_msg</span><span class="o">.</span><span class="n">header</span><span class="o">.</span><span class="n">frame_id</span> <span class="o">=</span> <span class="n">frame_id</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">height</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">width</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">fields</span> <span class="o">=</span> <span class="n">arr_to_fields</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">)</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">is_bigendian</span> <span class="o">=</span> <span class="kc">False</span> <span class="c1"># assumption</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">point_step</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">itemsize</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">row_step</span> <span class="o">=</span> <span class="n">cloud_msg</span><span class="o">.</span><span class="n">point_step</span><span class="o">*</span><span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">is_dense</span> <span class="o">=</span> <span class="nb">all</span><span class="p">([</span><span class="n">np</span><span class="o">.</span><span class="n">isfinite</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">[</span><span class="n">fname</span><span class="p">])</span><span class="o">.</span><span class="n">all</span><span class="p">()</span> <span class="k">for</span> <span class="n">fname</span> <span class="ow">in</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">names</span><span class="p">])</span>
    <span class="n">cloud_msg</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">tostring</span><span class="p">()</span></div>
    <span class="k">return</span> <span class="n">cloud_msg</span>

<div class="viewcode-block" id="merge_rgb_fields"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.merge_rgb_fields">[docs]</a><span class="k">def</span> <span class="nf">merge_rgb_fields</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Takes an array with named np.uint8 fields &#39;r&#39;, &#39;g&#39;, and &#39;b&#39;, and returns an array in</span>
<span class="sd">    which they have been merged into a single np.float32 &#39;rgb&#39; field. The first byte of this</span>
<span class="sd">    field is the &#39;r&#39; uint8, the second is the &#39;g&#39;, uint8, and the third is the &#39;b&#39; uint8.</span>

<span class="sd">    This is the way that pcl likes to handle RGB colors for some reason.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="n">r</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">[</span><span class="s1">&#39;r&#39;</span><span class="p">],</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint32</span><span class="p">)</span>
    <span class="n">g</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">[</span><span class="s1">&#39;g&#39;</span><span class="p">],</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint32</span><span class="p">)</span>
    <span class="n">b</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">[</span><span class="s1">&#39;b&#39;</span><span class="p">],</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint32</span><span class="p">)</span>
    <span class="n">rgb_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">((</span><span class="n">r</span> <span class="o">&lt;&lt;</span> <span class="mi">16</span><span class="p">)</span> <span class="o">|</span> <span class="p">(</span><span class="n">g</span> <span class="o">&lt;&lt;</span> <span class="mi">8</span><span class="p">)</span> <span class="o">|</span> <span class="p">(</span><span class="n">b</span> <span class="o">&lt;&lt;</span> <span class="mi">0</span><span class="p">),</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint32</span><span class="p">)</span>

    <span class="c1"># not sure if there is a better way to do this. i&#39;m changing the type of the array</span>
    <span class="c1"># from uint32 to float32, but i don&#39;t want any conversion to take place -jdb</span>
    <span class="n">rgb_arr</span><span class="o">.</span><span class="n">dtype</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span>

    <span class="c1"># create a new array, without r, g, and b, but with rgb float32 field</span>
    <span class="n">new_dtype</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">field_name</span> <span class="ow">in</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">names</span><span class="p">:</span>
        <span class="n">field_type</span><span class="p">,</span> <span class="n">field_offset</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">fields</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span>
        <span class="k">if</span> <span class="n">field_name</span> <span class="ow">not</span> <span class="ow">in</span> <span class="p">(</span><span class="s1">&#39;r&#39;</span><span class="p">,</span> <span class="s1">&#39;g&#39;</span><span class="p">,</span> <span class="s1">&#39;b&#39;</span><span class="p">):</span>
            <span class="n">new_dtype</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="n">field_name</span><span class="p">,</span> <span class="n">field_type</span><span class="p">))</span>
    <span class="n">new_dtype</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;rgb&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">))</span>
    <span class="n">new_cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">,</span> <span class="n">new_dtype</span><span class="p">)</span>

    <span class="c1"># fill in the new array</span>
    <span class="k">for</span> <span class="n">field_name</span> <span class="ow">in</span> <span class="n">new_cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">names</span><span class="p">:</span>
        <span class="k">if</span> <span class="n">field_name</span> <span class="o">==</span> <span class="s1">&#39;rgb&#39;</span><span class="p">:</span>
            <span class="n">new_cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span> <span class="o">=</span> <span class="n">rgb_arr</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">new_cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span>
</div>
    <span class="k">return</span> <span class="n">new_cloud_arr</span>

<div class="viewcode-block" id="split_rgb_field"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.split_rgb_field">[docs]</a><span class="k">def</span> <span class="nf">split_rgb_field</span><span class="p">(</span><span class="n">cloud_arr</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Takes an array with a named &#39;rgb&#39; float32 field, and returns an array in which</span>
<span class="sd">    this has been split into 3 uint 8 fields: &#39;r&#39;, &#39;g&#39;, and &#39;b&#39;.</span>

<span class="sd">    (pcl stores rgb in packed 32 bit floats)</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="n">rgb_arr</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="p">[</span><span class="s1">&#39;rgb&#39;</span><span class="p">]</span><span class="o">.</span><span class="n">copy</span><span class="p">()</span>
    <span class="n">rgb_arr</span><span class="o">.</span><span class="n">dtype</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">uint32</span>
    <span class="n">r</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">((</span><span class="n">rgb_arr</span> <span class="o">&gt;&gt;</span> <span class="mi">16</span><span class="p">)</span> <span class="o">&amp;</span> <span class="mi">255</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">)</span>
    <span class="n">g</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">((</span><span class="n">rgb_arr</span> <span class="o">&gt;&gt;</span> <span class="mi">8</span><span class="p">)</span> <span class="o">&amp;</span> <span class="mi">255</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">)</span>
    <span class="n">b</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">rgb_arr</span> <span class="o">&amp;</span> <span class="mi">255</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">)</span>

    <span class="c1"># create a new array, without rgb, but with r, g, and b fields</span>
    <span class="n">new_dtype</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">field_name</span> <span class="ow">in</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">names</span><span class="p">:</span>
        <span class="n">field_type</span><span class="p">,</span> <span class="n">field_offset</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">fields</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="n">field_name</span> <span class="o">==</span> <span class="s1">&#39;rgb&#39;</span><span class="p">:</span>
            <span class="n">new_dtype</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="n">field_name</span><span class="p">,</span> <span class="n">field_type</span><span class="p">))</span>
    <span class="n">new_dtype</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;r&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">))</span>
    <span class="n">new_dtype</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;g&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">))</span>
    <span class="n">new_dtype</span><span class="o">.</span><span class="n">append</span><span class="p">((</span><span class="s1">&#39;b&#39;</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">uint8</span><span class="p">))</span>
    <span class="n">new_cloud_arr</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="n">cloud_arr</span><span class="o">.</span><span class="n">shape</span><span class="p">,</span> <span class="n">new_dtype</span><span class="p">)</span>

    <span class="c1"># fill in the new array</span>
    <span class="k">for</span> <span class="n">field_name</span> <span class="ow">in</span> <span class="n">new_cloud_arr</span><span class="o">.</span><span class="n">dtype</span><span class="o">.</span><span class="n">names</span><span class="p">:</span>
        <span class="k">if</span> <span class="n">field_name</span> <span class="o">==</span> <span class="s1">&#39;r&#39;</span><span class="p">:</span>
            <span class="n">new_cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span> <span class="o">=</span> <span class="n">r</span>
        <span class="k">elif</span> <span class="n">field_name</span> <span class="o">==</span> <span class="s1">&#39;g&#39;</span><span class="p">:</span>
            <span class="n">new_cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span> <span class="o">=</span> <span class="n">g</span>
        <span class="k">elif</span> <span class="n">field_name</span> <span class="o">==</span> <span class="s1">&#39;b&#39;</span><span class="p">:</span>
            <span class="n">new_cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span> <span class="o">=</span> <span class="n">b</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">new_cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span> <span class="o">=</span> <span class="n">cloud_arr</span><span class="p">[</span><span class="n">field_name</span><span class="p">]</span></div>
    <span class="k">return</span> <span class="n">new_cloud_arr</span>

<div class="viewcode-block" id="get_xyz_points"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.get_xyz_points">[docs]</a><span class="k">def</span> <span class="nf">get_xyz_points</span><span class="p">(</span><span class="n">cloud_array</span><span class="p">,</span> <span class="n">remove_nans</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float</span><span class="p">):</span>
    <span class="sd">&#39;&#39;&#39;Pulls out x, y, and z columns from the cloud recordarray, and returns</span>
<span class="sd">	a 3xN matrix.</span>
<span class="sd">    &#39;&#39;&#39;</span>
    <span class="c1"># remove crap points</span>
    <span class="k">if</span> <span class="n">remove_nans</span><span class="p">:</span>
        <span class="n">mask</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">isfinite</span><span class="p">(</span><span class="n">cloud_array</span><span class="p">[</span><span class="s1">&#39;x&#39;</span><span class="p">])</span> <span class="o">&amp;</span> <span class="n">np</span><span class="o">.</span><span class="n">isfinite</span><span class="p">(</span><span class="n">cloud_array</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">])</span> <span class="o">&amp;</span> <span class="n">np</span><span class="o">.</span><span class="n">isfinite</span><span class="p">(</span><span class="n">cloud_array</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">])</span>
        <span class="n">cloud_array</span> <span class="o">=</span> <span class="n">cloud_array</span><span class="p">[</span><span class="n">mask</span><span class="p">]</span>

    <span class="c1"># pull out x, y, and z values</span>
    <span class="n">points</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="nb">list</span><span class="p">(</span><span class="n">cloud_array</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span> <span class="o">+</span> <span class="p">[</span><span class="mi">3</span><span class="p">],</span> <span class="n">dtype</span><span class="o">=</span><span class="n">dtype</span><span class="p">)</span>
    <span class="n">points</span><span class="p">[</span><span class="o">...</span><span class="p">,</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">cloud_array</span><span class="p">[</span><span class="s1">&#39;x&#39;</span><span class="p">]</span>
    <span class="n">points</span><span class="p">[</span><span class="o">...</span><span class="p">,</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">cloud_array</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">]</span>
    <span class="n">points</span><span class="p">[</span><span class="o">...</span><span class="p">,</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">cloud_array</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">]</span>
</div>
    <span class="k">return</span> <span class="n">points</span>

<div class="viewcode-block" id="pointcloud2_to_xyz_array"><a class="viewcode-back" href="../../pypcd.html#pypcd.numpy_pc2.pointcloud2_to_xyz_array">[docs]</a><span class="k">def</span> <span class="nf">pointcloud2_to_xyz_array</span><span class="p">(</span><span class="n">cloud_msg</span><span class="p">,</span> <span class="n">remove_nans</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span></div>
    <span class="k">return</span> <span class="n">get_xyz_points</span><span class="p">(</span><span class="n">pointcloud2_to_array</span><span class="p">(</span><span class="n">cloud_msg</span><span class="p">))</span>
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper"><div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../index.html">Documentation overview</a><ul>
  <li><a href="../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3>Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../search.html" method="get">
      <input type="text" name="q" />
      <input type="submit" value="Go" />
      <input type="hidden" name="check_keywords" value="yes" />
      <input type="hidden" name="area" value="default" />
    </form>
    </div>
</div>
<script type="text/javascript">$('#searchbox').show(0);</script>
        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2018, Daniel Maturana.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 1.7.1</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>